#include "movseq.h"
#include <math.h>

#ifdef DEBUG
#include "motrecreader.h"
#endif

#define INTSTRBUFSZ		1024
#define square(x)	((x)*(x))

MovementSequence::MovementSequence() {
	_mov_seq.clear();
	_font = cvFont(3, 5);
}

MovementSequence::~MovementSequence() {
	_mov_seq.clear();
}

static int arrowline(IplImage *img, CvPoint p, CvPoint q) {
	double angle;
	double pi = 3.1415926;
	double len = 15;
	angle = atan2((double) p.y - q.y, (double) p.x - q.x);

	cvLine(img, p, q, CV_RGB(255,0,0), 5, CV_AA, 0);

	p.x = (int)(q.x + len * cos(angle + pi /4));
	p.y = (int)(q.y + len * sin(angle + pi /4));
	cvLine(img, p, q, CV_RGB(255,0,0), 5, CV_AA, 0);
	p.x = (int)(q.x + len * cos(angle - pi /4));
	p.y = (int)(q.y + len * sin(angle - pi /4));
	cvLine(img, p, q, CV_RGB(255,0,0), 5, CV_AA, 0);
	return 0;
}

int MovementSequence::draw_mov(Movement &mov, 
		IplImage *img, 
		int current_time) {
	char str[INTSTRBUFSZ];

	int time_left;
	if (mov.points_on_img.size() <= 1)
		return -1;
	std::list<CvPoint>::iterator last = mov.points_on_img.begin();
	std::list<CvPoint>::iterator iter = mov.points_on_img.begin();
	CvScalar color = CV_RGB(255, 0, 0);
	int thick = 5;
	int nr_point = mov.points_on_img.size();
	for (++iter; iter != mov.points_on_img.end(); ++iter) {
		printf("Draw Line from (%d, %d) to (%d, %d)\n",
				last->x, last->y, iter->x, iter->y);
		if (--nr_point > 1)
			cvLine(img, *last, *iter, color, thick);
		else
			arrowline(img, *last, *iter);
		last = iter;
	}
	time_left = mov.start_time + mov.duration - current_time;
	memset(str, 0, INTSTRBUFSZ);
	snprintf(str, INTSTRBUFSZ, "%ds", time_left);
	cvPutText(img, str, *last, &_font, CV_RGB(255,0,0));	
	return 0;
}

int MovementSequence::draw_movs(int current_time, IplImage *img) {
	trim(current_time);
	for (std::list<std::pair<int, Movement> >::iterator iter = _mov_seq.begin();
			iter != _mov_seq.end(); ++iter) {
		draw_mov(iter->second, img, current_time);
	}
	return 0;
}

int MovementSequence::trim(int current_time) {
	int exp;
	if (_mov_seq.size() > 0) {
		for (exp = _mov_seq.front().first;
				exp <= current_time;
				exp = _mov_seq.front().first) {
			_mov_seq.pop_front();
			if (0 == _mov_seq.size())
				break;
		}
	}
	return 0;
}

int MovementSequence::insert_mov(int current_time, Movement &mov) {
	int exp;

	if (0 == _mov_seq.size()) {
		exp = mov.start_time + mov.duration;
		_mov_seq.push_back(std::pair<int, Movement>(exp, mov));
		return 0;
	}
	exp = mov.start_time + mov.duration;
	for (std::list<std::pair<int, Movement> >::iterator iter = _mov_seq.begin();
			iter != _mov_seq.end(); ++iter) {
		if (iter->first < exp)
			continue;
		_mov_seq.insert(iter, std::pair<int, Movement>(exp, mov));
		return 0;
	}
	_mov_seq.push_back(std::pair<int, Movement>(exp, mov));
	return 0;
}

int MovementSequence::update(int current_time, Movement &mov) {
	int exp;

	if (0 == _mov_seq.size()) {
		exp = mov.start_time + mov.duration;
		_mov_seq.push_back(std::pair<int, Movement>(exp, mov));
		return 0;
	}
	for (exp = _mov_seq.front().first; 
			exp <= current_time; 
			exp = _mov_seq.front().first) {
		_mov_seq.pop_front();
		if (0 == _mov_seq.size())
			break;
	}
	exp = mov.start_time + mov.duration;
	for (std::list<std::pair<int, Movement> >::iterator iter = _mov_seq.begin();
			iter != _mov_seq.end(); ++iter) {
		if (iter->first < exp)
			continue;
		_mov_seq.insert(iter, std::pair<int, Movement>(exp, mov));
		return 0;
	}
	_mov_seq.push_back(std::pair<int, Movement>(exp, mov));
	return 0;
}

int MovementSequence::update(int current_time, std::list<Movement> &new_movs) {
	int exp;

	if (_mov_seq.size() > 0) {
		for (exp = _mov_seq.front().first;
				exp <= current_time;
				exp = _mov_seq.front().first) {
			_mov_seq.pop_front();
			if (0 == _mov_seq.size())
				break;
		}
	}
	for (std::list<Movement>::iterator iter = new_movs.begin();
			iter != new_movs.end(); ++iter) {
		insert_mov(current_time, *iter);
	}
	return 0;
}

#ifdef DEBUG
int MovementSequence::print_mov_seq(void) {
	for (std::list<std::pair<int, Movement> >::iterator iter = _mov_seq.begin();
			iter != _mov_seq.end(); ++iter) {
		printf("expire time: %ds\n", iter->first);
		print_movement(iter->second);
	}
}
#endif

#ifdef TEST_MOVSEQ

#define MOTFILE	"example.mot"

int fake_coord(Movement &mov) {
	int unit = 100;
	CvPoint p;
	for (std::list<std::pair<double, double> >::iterator iter = mov.points.begin();
			iter != mov.points.end(); ++iter) {
		p.x = (int)iter->first * unit;
		p.y = (int)iter->second * unit;
		mov.points_on_img.push_back(p);
	}
	return 0;
}

int fake_coord(std::list<Movement> &mov) {
	for (std::list<Movement>::iterator iter = mov.begin();
			iter != mov.end(); ++iter) {
		fake_coord(*iter);
	}
	return 0;
}

int main() {
	MotionRecordReader mrr(MOTFILE);
	MovementSequence movseq;
	std::list<Movement> mov_list;
	int current_time;
	int ret;

	for (current_time = 0; current_time < 10; ++current_time) {
		printf("===========================\n");
		printf("Current Time: %d\n", current_time);
		mov_list.clear();
		ret = mrr.read_next_movement_list(mov_list, current_time);
		fake_coord(mov_list);
		movseq.update(current_time, mov_list);
		//movseq.print_mov_seq();
		if (0 == movseq.size() && ret < 0)
			break;
		movseq.draw_movs(current_time, NULL);
	}
	printf("Finish\n");
	return 0;
}
#endif

